Distributed Spatial Awareness for Robot Swarms

Uses Gaussian Belief Propagation to build a shared model of the positions of each robot and items to be delivered.

Each robot maintains a local factor graph with variables representing 2D pose, unary anchor factors connecting to a single variable, and binary relative measurement factors connecting two variables.

Each robot also relies on a sensor called an odometer which can keep track of its relative position changes over time.

Each robot shares synchronised time, and at regular intervals a new timestamp $ts_i$ is issued, and factor graphs are updated with a new variable node $x_i$ representing the estimated position of the robot at timestamp $ts_i$.

The first node to be created will have an anchor factor connected to it with a weak prior pose of (0, 0) indicating the robot does not know where it is, so is just guessing (0, 0).

Successive nodes will have a measurement factor connecting them to the previous node. The factor utilises the odometer to provide this measurement indicating how far the robot has moved since the previous node.

Over time, older nodes are pruned to maintain a certain number of nodes. The oldest node at any given time is connected to an anchor node containing the belief of that variable.

Each timestep, a factor node is chosen at random and propagation is done outwards from that node. Robots then also observe other nearby robots and add these measurements to their factor graphs.

Each robot picks one other nearby robot and sends a message exchange request. This consists of a list of the timesteps where the current robot could see the other robot. The other robot then replies with its belief of its own position at those timesteps. It also replies with a Factor-to-variable message for each factor it has in its own factor graph which connects to a variable in the current robots factor graph.